from scipy.optimize import linear_sum_assignment
import numpy as np

# 显示时的放大倍率,因为按照题目大小单位，1像素的小车根本看不到,必须放大
up_sample = 1
# 点之间的间隔
distance = 32 * up_sample

# 图像大小
width = 200 * up_sample
height = 200 * up_sample

yaml = ''

# 起始点坐标
yaml += 'agents:'

# 点之间的间隔
distance = 32

# 先前的坐标
points_start = []
for i in range(6):
    for j in range(6):
        points_start.append([20 + distance * i, 20 + distance * j])
# 之后的坐标
points_goal = [[87, 157], [72, 172], [53, 180], [32, 177], [16, 165], [10, 145], [17, 125], [33, 110], [50, 100],
               [68, 88], [84, 74], [90, 54], [84, 35], [66, 22], [45, 20], [26, 28], [13, 44],
               [110, 175], [110, 153.5], [110, 132], [110, 110.6], [110, 89.2], [110, 67.8], [110, 46.4], [110, 25],
               [128.6, 100], [150, 100], [171.4, 100],
               [190, 175], [190, 153.5], [190, 132], [190, 110.6], [190, 89.2], [190, 67.8], [190, 46.4], [190, 25],
               ]
angles = [150, 120, 100, 60, 45, 0, -30, -45, -45, -45, -30, 0, 45, 60, 100, 135, 150,
          0, 0, 0, 0, 0, 0, 0, 0, 90, 90, 90, 0, 0, 0, 0, 0, 0, 0, 0,
          ]
# 使用坐标计算代价矩阵
cost_matrix = [[np.power((np.array(a) - np.array(b)), 2).sum() for a in points_start] for b in points_goal]
# 进行匈牙利算法匹配
row_ind, col_ind = linear_sum_assignment(cost_matrix)
for x, y in zip(row_ind, col_ind):
    # print("起始列表中的%s,应该与结束列表中坐标%s匹配,距离消耗为%d" % (points_start[y], points_goal[x], cost_matrix[x][y]))
    num = y
    point = [points_start[y][0], points_start[y][1], -1.57]
    goal_point = [int(points_goal[x][0]), int(points_goal[x][1]), 3.14*(angles[x]-90)/180]
    yaml += f'''
  - start: {point}
    name: agent{num}
    goal: {goal_point}'''

# 图像大小
yaml += f'''
map:
  dimensions: [{width}, {height}]
  obstacles: 
    - [-1, -1]'''

# 写入
with open('question2.yaml', mode='w') as f:
    f.write(yaml)
